/**
 * @file /include/alphadog_monitor/qnode.hpp
 *
 * @brief Communications central!
 *
 * @date February 2011
 **/
/*****************************************************************************
** Ifdefs
*****************************************************************************/

#ifndef alphadog_monitor_QNODE_HPP_
#define alphadog_monitor_QNODE_HPP_

/*****************************************************************************
** Includes
*****************************************************************************/

// To workaround boost/qt4 problems that won't be bugfixed. Refer to
//    https://bugreports.qt.io/browse/QTBUG-22829
#ifndef Q_MOC_RUN
#include <ros/ros.h>
#endif
#include <string>
#include <QThread>
#include <QStringListModel>
#include <QSettings>
#include <QImage>
#include <sensor_msgs/JointState.h>
#include <sensor_msgs/CompressedImage.h>
#include <sensor_msgs/Imu.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/PointStamped.h>
#include <sensor_msgs/JointState.h>
#include <cv_bridge/cv_bridge.h>
#include <vector>
#include <trajectory_msgs/JointTrajectory.h>



/*****************************************************************************
** Namespaces
*****************************************************************************/

namespace alphadog_monitor {

/*****************************************************************************
** Class
*****************************************************************************/

class QNode : public QThread {
    Q_OBJECT
public:
	QNode(int argc, char** argv );
	virtual ~QNode();
	bool init();
	bool init(const std::string &master_url, const std::string &host_url);
	void run();

	/*********************
	** Logging
	**********************/
	enum LogLevel {
	         Debug,
	         Info,
	         Warn,
	         Error,
	         Fatal
	 };

	QStringListModel* loggingModel() { return &logging_model; }
	void log( const LogLevel &level, const std::string &msg);

    ros::Publisher JointCmdPuber;          // 关节指令发布者
    ros::Publisher FootCmdPuber;           // 足端指令发布者
    ros::Publisher StatusCmdPuber;         // 状态命令发布者
    ros::Publisher JoystickCmdPuber;       // 遥控器指令发布者
    ros::Publisher RouteDataPuber;        // 路径规划数据发布者

    ros::Subscriber InitImgSuber;         // 原始图像订阅者
    ros::Subscriber HandledImgSuber;      // 处理后的图像订阅者
    ros::Subscriber ImuSuber;             // 姿态订阅者
    ros::Subscriber FeetSuber;            // 足端位置订阅者
    ros::Subscriber GraphSuber;           // 曲线数据订阅者
    ros::Subscriber StateSuber;           // 机器人姿态和位置订阅者

Q_SIGNALS:
	void loggingUpdated();
    void rosShutdown();
    void Show_image(int, QImage);
    void updateGraph(float*, QString*);
    void updateState(float*);

private:
	int init_argc;
	char** init_argv;
    bool enabled = false;   // 用于控制上位机信号是否发出
    float state[6];
    float debug_val[12];
    QString debug_name[12];


    QStringListModel logging_model;
    QString JointCmdTopic, FootCmdTopic, JoystickCmdTopic, StatusCmdTopic;
    QString InitImgTopic, HandledImgTopic, ImuDataTopic, FeetDataTopic;
    QString GraphTopic;

private:
    void SubAndPubTopic();

    // 图像回调函数
    void InitImgCallback(const sensor_msgs::CompressedImageConstPtr& msg);
    void HandledImgCallback(const sensor_msgs::CompressedImageConstPtr& msg);

    // 反馈数据回调函数
    void FeetDataReceiveCallback(const sensor_msgs::JointStatePtr& msg);
    void ImuDataReceiveCallback(const sensor_msgs::ImuPtr& msg);
    void GraphDataReceiveCallback(const sensor_msgs::JointStateConstPtr& msg);
    void StateDataCallback(const sensor_msgs::JointStateConstPtr& msg);

    // 发送指令
    void SendJointCmd();
    void SendFootCmd();
    void SendGaitCmd();

    QImage Mat2QImage(cv::Mat const& src);
    cv::Mat QImage2Mat(QImage& image);
};

}  // namespace alphadog_monitor

#endif /* alphadog_monitor_QNODE_HPP_ */
